#!/usr/bin/env python
# coding: utf-8

# In[ ]:


import cv2
import numpy as np
import math
import serial
import RPi.GPIO as GPIO
import time
import enum
import ipywidgets.widgets as widgets
import threading
import inspect
import ctypes


def Routing(point1, point2, remap_point, image):
    p1 = remap_point[point1[0]][point1[1]]
    p2 = remap_point[point2[0]][point2[1]]
    # cv2.circle(im_crop, p1, 2, (0, 0, 255), 2)  # 画圆心
    # cv2.circle(im_crop, p2, 2, (0, 0, 255), 2)  # 画圆心
    # 如果x轴相等，那就是y轴需要移动
    if (p1[0] == p2[0]):
        if p2[1] > p1[1]:
            a = image[p1[1]:p2[1], p1[0]] > 0
        else:
            a = image[p2[1]:p1[1], p1[0]] > 0
        sum = np.sum(a)
        if sum > 0:
            return False
    if (p1[1] == p2[1]):
        if (p1[0] > p2[0]):
            a = image[p1[1], p2[0]:p1[0]] > 0
        else:
            a = image[p1[1], p1[0]:p2[0]] > 0
        sum = np.sum(a)
        if sum > 0:
            return False
    return True


def get_route(point0, point1, im_crop, edge, points):
    bianli_list = []
    route = []
    head = 0  # 队列头
    # rear = 0#队列尾巴
    bianli_list.append([point0[0], point0[1], -1, 1])
    while bianli_list[head][0] != point1[0] or bianli_list[head][1] != point1[1]:
        x = bianli_list[head][0]
        y = bianli_list[head][1]
        r = bianli_list[head][3]
        point_lf = [x - 1, y]
        point_rg = [x + 1, y]
        point_up = [x, y - 1]
        point_dn = [x, y + 1]
        if 0 <= point_lf[0] < 10 and 0 <= point_lf[1] < 10:
            if point_lf[0] != bianli_list[bianli_list[head][2]][0] or point_lf[1] != bianli_list[bianli_list[head][2]][
                1]:
                if Routing([x, y], point_lf, points, edge) == True:
                    bianli_list.append([point_lf[0], point_lf[1], head, r + 1])

        if 0 <= point_rg[0] < 10 and 0 <= point_rg[1] < 10:
            if point_rg[0] != bianli_list[bianli_list[head][2]][0] or point_rg[1] != bianli_list[bianli_list[head][2]][
                1]:
                if Routing([x, y], point_rg, points, edge) == True:
                    bianli_list.append([point_rg[0], point_rg[1], head, r + 1])

        if 0 <= point_up[0] < 10 and 0 <= point_up[1] < 10:
            if point_up[0] != bianli_list[bianli_list[head][2]][0] or point_up[1] != bianli_list[bianli_list[head][2]][
                1]:
                if Routing([x, y], point_up, points, edge) == True:
                    bianli_list.append([point_up[0], point_up[1], head, r + 1])

        if 0 <= point_dn[0] < 10 and 0 <= point_dn[1] < 10:
            if point_dn[0] != bianli_list[bianli_list[head][2]][0] or point_dn[1] != bianli_list[bianli_list[head][2]][
                1]:
                if Routing([x, y], point_dn, points, edge) == True:
                    bianli_list.append([point_dn[0], point_dn[1], head, r + 1])
        head += 1

    while (bianli_list[head][2] != -1):
        x0 = bianli_list[head][0]
        y0 = bianli_list[head][1]
        route.append([x0, y0])
        pre_index = bianli_list[head][2]
        x1 = bianli_list[pre_index][0]
        y1 = bianli_list[pre_index][1]
        cv2.line(im_crop, (points[x0][y0][0], points[x0][y0][1]), (points[x1][y1][0], points[x1][y1][1]), (0, 0, 255),
                 2)
        head = pre_index
    return route
    # cv2.imshow("line",im_crop)
    # cv2.waitKey()


def get_short_route(point0, baozang_points, im_crop, edge, points):
    bianli_list = []
    head = 0  # 队列头
    route = []
    total_route = []
    # rear = 0#队列尾巴
    bianli_list.append([point0[0], point0[1], -1, 1])
    while (baozang_points):
        while [bianli_list[head][0], bianli_list[head][1]] not in baozang_points:
            x = bianli_list[head][0]
            y = bianli_list[head][1]
            r = bianli_list[head][3]
            point_lf = [x - 1, y]
            point_rg = [x + 1, y]
            point_up = [x, y - 1]
            point_dn = [x, y + 1]
            if 0 <= point_lf[0] < 10 and 0 <= point_lf[1] < 10:
                if point_lf[0] != bianli_list[bianli_list[head][2]][0] or point_lf[1] != \
                        bianli_list[bianli_list[head][2]][1]:
                    if Routing([x, y], point_lf, points, edge) == True:
                        bianli_list.append([point_lf[0], point_lf[1], head, r + 1])

            if 0 <= point_rg[0] < 10 and 0 <= point_rg[1] < 10:
                if point_rg[0] != bianli_list[bianli_list[head][2]][0] or point_rg[1] != \
                        bianli_list[bianli_list[head][2]][1]:
                    if Routing([x, y], point_rg, points, edge) == True:
                        bianli_list.append([point_rg[0], point_rg[1], head, r + 1])

            if 0 <= point_up[0] < 10 and 0 <= point_up[1] < 10:
                if point_up[0] != bianli_list[bianli_list[head][2]][0] or point_up[1] != \
                        bianli_list[bianli_list[head][2]][1]:
                    if Routing([x, y], point_up, points, edge) == True:
                        bianli_list.append([point_up[0], point_up[1], head, r + 1])

            if 0 <= point_dn[0] < 10 and 0 <= point_dn[1] < 10:
                if point_dn[0] != bianli_list[bianli_list[head][2]][0] or point_dn[1] != \
                        bianli_list[bianli_list[head][2]][1]:
                    if Routing([x, y], point_dn, points, edge) == True:
                        bianli_list.append([point_dn[0], point_dn[1], head, r + 1])
            head += 1
        new_start_point = [bianli_list[head][0], bianli_list[head][1]]
        baozang_points.remove(new_start_point)
        while (bianli_list[head][2] != -1):
            x0 = bianli_list[head][0]
            y0 = bianli_list[head][1]
            route.append([x0, y0])
            pre_index = bianli_list[head][2]
            x1 = bianli_list[pre_index][0]
            y1 = bianli_list[pre_index][1]
            cv2.line(im_crop, (points[x0][y0][0], points[x0][y0][1]), (points[x1][y1][0], points[x1][y1][1]),
                     (51, 255, 255), 2)  # BGR黄色
            head = pre_index
        route.append([bianli_list[head][0], bianli_list[head][1]])
        total_route.append(route[:])
        route.clear()
        bianli_list.clear()
        bianli_list.append([new_start_point[0], new_start_point[1], -1, 1])
    route = get_route([9, 0], new_start_point, im_crop, edge, points)
    total_route.append(route)
    return im_crop, total_route


def find_rect(img):
    edge = cv2.Canny(img, 200, 250);
    # binaly_img   = cv2.dilate(edge, kernel, iterations = 1)
    contours, hierarchy = cv2.findContours(edge, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE, )

    # 3print("-----------------------------")
    # print((contours[2]))
    bounding_boxes = [cv2.boundingRect(cnt) for cnt in contours]
    index_list = []
    # print(contours[2][0])
    # 找到有内轮廓的轮廓
    for i in range(len(bounding_boxes)):
        hie = hierarchy[0][i]
        j = 0;
        while (hie[2] > 0):
            j += 1
            hie = hierarchy[0][hie[2]]
        if j > 2:
            index_list.append(i)

    for i, box in enumerate(bounding_boxes):
        if i in index_list:
            if box[2] > 1.5 * box[3] or box[2] < 0.6 * box[3] or box[3] * box[2] > 10000:
                index_list.remove(i)

            # else:
            #  cv2.rectangle(img, (box[0], box[1]), (box[0] + box[2], box[1] + box[3]), (0, 255, 0), 2)

    nei_list = []
    for i in index_list:
        hie = hierarchy[0][i]
        j = 0;
        while (hie[2] > 0):
            hie = hierarchy[0][hie[2]]
            h1 = bounding_boxes[i][3]
        nei_list.append(hie[3])

    nei_list.sort()

    index_list = []
    for l in nei_list:
        if l not in index_list:
            index_list.append(l)
            box = bounding_boxes[l]
            cv2.rectangle(img, (box[0], box[1]), (box[0] + box[2], box[1] + box[3]), (0, 255, 0), 2)

    points = []
    cv2.imshow("img", img)
    # cv2.waitKey(10)
    if len(index_list) >= 4:
        for l in index_list:
            x, y, w, h = bounding_boxes[l]
            if w * h < 400:
                points.append([int(x + w / 2), int(y + h / 2)])

    if (len(points) == 4):
        points.sort()
        if (points[0][1] < points[1][1]):
            left_top_p = points[0]
            left_down_p = points[1]
        else:
            left_top_p = points[1]
            left_down_p = points[0]

        if (points[2][1] < points[3][1]):
            right_top_p = points[2]
            right_down_p = points[3]
        else:
            right_top_p = points[3]
            right_down_p = points[2]

        return left_top_p, left_down_p, right_top_p, right_down_p, h1
    return 0, 0, 0, 0, 0


def img_jiaozheng(left_top_p, left_down_p, right_top_p, right_down_p, frame, h1):
    # if (right_down_p[0]-left_top_p[0])>(right_down_p[1]-left_top_p[1]):
    #  w = right_down_p[0]-left_top_p[0]
    # else:
    # w = right_down_p[1]-left_top_p[1]
    tl = left_top_p
    tr = right_top_p
    br = right_down_p
    bl = left_down_p

    widthA = np.sqrt(((br[0] - bl[0]) ** 2) + ((br[1] - bl[1]) ** 2))
    widthB = np.sqrt(((tr[0] - tl[0]) ** 2) + ((tr[1] - tl[1]) ** 2))
    maxWidth = max(int(widthA), int(widthB))

    heightA = np.sqrt(((tr[0] - br[0]) ** 2) + ((tr[1] - br[1]) ** 2))
    heightB = np.sqrt(((tl[0] - bl[0]) ** 2) + ((tl[1] - bl[1]) ** 2))
    maxheight = max(int(heightA), int(heightB))

    target_lf_t = [0, 0]
    target_rg_t = [maxWidth - 1, 0]
    target_rg_d = [maxWidth - 1, maxheight - 1]
    target_lf_d = [0, maxheight - 1]

    # 开始校正
    pointSrc = np.float32([tl, tr, br, bl])  # 原始图像中 4点坐标
    pointDst = np.float32([target_lf_t, target_rg_t, target_rg_d, target_lf_d])  # 变换图像中 4点坐标
    MP = cv2.getPerspectiveTransform(pointSrc, pointDst)  # 计算投影变换矩阵 M
    imgP = cv2.warpPerspective(frame, MP, (maxWidth, maxheight))
    im_crop = imgP[int(maxheight / 22):maxheight - int(maxheight / 22),
              int(maxWidth / 22):maxWidth - int(maxWidth / 22), :]

    cv2.imshow("img", im_crop)  # 矫正后的图片
    cv2.waitKey(10)

    return im_crop


def get_points(im_crop):
    gray_image = cv2.cvtColor(im_crop, cv2.COLOR_RGB2GRAY)
    ret, edge = cv2.threshold(gray_image, 70, 255, cv2.THRESH_BINARY_INV)
    # edge =cv2.Canny(gray_image,200,250)+edge;
    kernel = np.ones((3, 3), np.uint8)
    baozang_points = []
    points = []
    edge = cv2.dilate(edge, kernel, iterations=1)

    kernel = np.ones((1, int(im_crop.shape[1] / 30)), np.uint8)
    edge1 = cv2.morphologyEx(edge, cv2.MORPH_OPEN, kernel, iterations=2);
    lines = cv2.HoughLines(edge1, 1, np.pi / 2, 80, 200)
    mid_p_x = []
    if 1:
        lines1 = lines[:, 0, :]
        # mid_p_x = []
        for rho, theta in lines1[:]:
            a = np.cos(theta)
            b = np.sin(theta)
            y0 = b * rho
            if y0 > 0:
                mid_p_x.append(y0)

    kernel = np.ones((int(im_crop.shape[0] / 30), 1), np.uint8)
    binaly_img2 = cv2.morphologyEx(edge, cv2.MORPH_OPEN, kernel, iterations=2);
    lines = cv2.HoughLines(binaly_img2, 1, np.pi / 2, 50, 150)
    # lines1 = lines[:, 0, :]
    mid_p_y = []
    if 1:

        lines1 = lines[:, 0, :]
        for rho, theta in lines1[:]:
            a = np.cos(theta)
            b = np.sin(theta)
            x0 = a * rho
            if x0 > 0:
                mid_p_y.append(x0)

    if (len(mid_p_y) > 5 and len(mid_p_x) > 5):
        mid_p_y.sort()
        mid_p_x.sort()
        len_x = len(mid_p_x)
        len_y = len(mid_p_y)
        gap_x = []
        gap_y = []
        for i in range(len_x - 1):
            gap_x.append(mid_p_x[i + 1] - mid_p_x[i])
        for i in range(len_y - 1):
            gap_y.append(mid_p_y[i + 1] - mid_p_y[i])
        width, height, c = im_crop.shape
        gap_av_x = width / 12  # np.mean(gap_x)
        gap_av_y = height / 12  # np.mean(gap_y)
        remove_x = []
        remove_y = []
        for i, g in enumerate(gap_x):
            if g < gap_av_x * 0.7:
                remove_x.append(mid_p_x[i + 1])
            if g > gap_av_x * 1.5:
                mid_p_x.append(mid_p_x[i] + int((mid_p_x[i + 1] - mid_p_x[i]) / 2))
        for i, g in enumerate(gap_y):
            if g < gap_av_y * 0.7:
                remove_y.append(mid_p_y[i + 1])
            if g > gap_av_y * 1.5:
                mid_p_y.append(mid_p_y[i] + int((mid_p_y[i + 1] - mid_p_y[i]) / 2))
        for x in remove_x:
            mid_p_x.remove(x)
        for y in remove_y:
            mid_p_y.remove(y)
        im = im_crop.copy()
        for x in mid_p_y:
            cv2.line(im, (int(x), 0), (int(x), 600), (0, 0, 255), 2)
        for x in mid_p_x:
            cv2.line(im, (0, int(x)), (600, int(x)), (0, 0, 255), 2)

        if len(mid_p_x) == 11 and len(mid_p_y) == 11:
            points = []
            mid_p_y.sort();
            mid_p_x.sort();
            for i in range(10):
                x_list = []
                y = mid_p_y[i]
                for j in range(10):
                    x = mid_p_x[j]
                    x_list.append([int(y + gap_av_y / 2), int(x + gap_av_x / 2)])
                    # cv2.circle(im_crop, (int(y+gap_av_y/2),int(x+gap_av_x/2)), 3, [0, 0, 255])

                points.append(x_list)
            kenel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (int(gap_av_y / 3), int(gap_av_x / 3)))
            yuan = cv2.morphologyEx(edge, cv2.MORPH_OPEN, kenel, iterations=1);
            # circles = cv2.HoughCircles(edge, cv2.HOUGH_GRADIENT, 1, 30, param1=100, param2=200, minRadius=0, maxRadius=0)
            contours, hierarchy = cv2.findContours(yuan, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
            bounding_boxes = [cv2.boundingRect(cnt) for cnt in contours]

            for bbox in bounding_boxes:
                [x, y, w, h] = bbox

                # baozang_point.append([int(x+w/2),int(y+h/2)])
                if (x > mid_p_y[0]) and ((x + w / 2) < mid_p_y[10]):
                    for i in range(10):
                        if (x - points[i][0][0]) < gap_av_y / 2:
                            break

                    for j in range(10):
                        if (y - points[0][j][1]) < gap_av_x / 2:
                            baozang_points.append([i, j]);
                            break

                    for p in baozang_points:
                        x = points[p[0]][p[1]]
                        cv2.circle(im, (x[0], x[1]), 5, [255, 0, 0])
        else:
            kenel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (int(gap_av_y / 3), int(gap_av_x / 3)))
            yuan = cv2.morphologyEx(edge, cv2.MORPH_OPEN, kenel, iterations=1);


    else:
        kenel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (int(30 / 3), int(30 / 3)))
        yuan = cv2.morphologyEx(edge, cv2.MORPH_OPEN, kenel, iterations=1);

        # return binaly_img2+edge1,points,baozang_point
        # cv2.imshow("yuan",yuan)
        # cv2.waitKey(10)
    kenel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3, 3))
    edge1 = binaly_img2 + edge1 + cv2.morphologyEx(edge - yuan, cv2.MORPH_OPEN, kenel, iterations=1);
    # edge1 = yuan
    # cv2.imshow('shange',im) # 迷宫格
    cv2.waitKey(10)
    # cv2.imshow("edge1",edge1) #边缘图像
    cv2.waitKey(10)
    # cv2.circle(im_crop, (int(y+gap_av_y/2),int(x+gap_av_x/2)), 5, [0, 0, 255])
    # edge2=binaly_img2+edge1
    return points, baozang_points, edge1

    # cv2.line(im_crop, (mid_p_y[0], 0), (mid_p_y[0], 600), (0, 0, 255), 2)

    # cv2.line(im_crop, (mid_p_y[10], 0), (mid_p_y[10], 600), (0, 0, 255), 2)


def del_img():
    kernel = np.ones((3, 3), np.uint8)
    cap = cv2.VideoCapture(0)  # 调用摄像头‘0’一般q是打开电脑自带摄像头，‘1’是打开外部摄像头（只有一个摄像头的情况）
    width = 800
    height = 600
    cap.set(cv2.CAP_PROP_FRAME_WIDTH, width)  # 设置图像宽度
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height)  # 设置图像高度
    # 显示图像
    while True:
        ret, frame = cap.read()  # 读取图像(frame就是读取的视频帧，对frame处理就是对整个视频的处理)
        # print(ret)#
        #######例如将图像灰度化处理，
        img = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)  # 转灰度图
        # cv2.imshow("imcrop",img)
        input = cv2.waitKey(10)
        # gray_image = cv2.medianBlur(img,3)
        left_top_p, left_down_p, right_top_p, right_down_p, h1 = find_rect(img)
        if (left_top_p != 0):
            im_crop = img_jiaozheng(left_top_p, left_down_p, right_top_p, right_down_p, frame, h1)
            cv2.imshow("jiaozheng", im_crop)  # 矫正后的图片
            input = cv2.waitKey(10)
            points, baozang_point, edge = get_points(im_crop)
            if len(baozang_point) == 8:
                return edge, points, baozang_point, im_crop
        # return 0,0,0


def route_convert_instruct(route):
    instruct_t = []
    instruct = []
    instruct.append(3)
    pre_dir = [1, 0]
    for r in route:
        instruct = []
        length = len(r)
        for i in range(2, len(r)):
            x = [r[length - i - 1][0] - r[length - i][0], r[length - i - 1][1] - r[length - i][1]]
            if (x == pre_dir):
                instruct.append(3)
            elif (x[0] < 0 and pre_dir[1] > 0):
                instruct.append(0)
            elif (x[0] < 0 and pre_dir[1] < 0):
                instruct.append(1)
            elif (x[0] > 0 and pre_dir[1] > 0):
                instruct.append(1)
            elif (x[0] > 0 and pre_dir[1] < 0):
                instruct.append(0)
            elif (x[1] < 0 and pre_dir[0] < 0):
                instruct.append(0)
            elif (x[1] < 0 and pre_dir[0] > 0):
                instruct.append(1)
            elif (x[1] > 0 and pre_dir[0] > 0):
                instruct.append(0)
            elif (x[1] > 0 and pre_dir[0] < 0):
                instruct.append(1)
            pre_dir = x
        # instruct.append(3)
        instruct.append(2)
        pre_dir = [-pre_dir[0], -pre_dir[1]]
        instruct_t.append(instruct[:])
        instruct.clear()
    return instruct_t


def send_data(ser, ins):
    for d in ins:
        for i in d:
            if (i == 0):
                ser.write(b'0')  # turn right
            if (i == 1):
                ser.write(b'1')  # turn left
            if (i == 2):
                ser.write(b'2')  # turn back
            if (i == 3):
                ser.write(b'3')  # go farward

        ser.write(b'4')  # shibiebaozang

    ser.write(b'5')  # tingche

def main():
    # ser = serial.Serial("/dev/ttyAMA0",115200) #启动串口
    # ser.write(b'')
    edge, points, baozang_point, im_crop = del_img()

    all_instructions = []  # 存储每个宝藏点的指令列表

    for point in baozang_point:
        if (len(baozang_point) == 8):
            im_crop1, total_r = get_short_route([0, 9], baozang_point, im_crop, edge, points)
            ins = route_convert_instruct(total_r)
            # send_data(ser,ins)  #通过串口发送数据
            all_instructions.append(ins)  # 存储当前宝藏点的指令列表
            print(ins)

    # 输出每个宝藏点的指令列表
    for i, instructions in enumerate(all_instructions):
        print(f"第 {i + 1} 个宝藏点的指令：")
        for j, ins in enumerate(instructions):
            print(f"turn_directions {j}： {ins}")

    while (True):
        cv2.imshow('lujing', im_crop1)  # 路径显示
        input = cv2.waitKey(10)
        if input == ord('q'):
            break
    # cap.release()#释放摄像头

    # cv2.destroyAllWindows()#销毁窗口


# 代码二中的函数和内容

# 线程相关函数
def _async_raise(tid, exctype):
    """raises the exception, performs cleanup if needed"""
    tid = ctypes.c_long(tid)
    if not inspect.isclass(exctype):
        exctype = type(exctype)
    res = ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, ctypes.py_object(exctype))
    if res == 0:
        raise ValueError("invalid thread id")
    elif res != 1:
        # """if it returns a number greater than one, you're in trouble,
        # and you should call it again with exc=NULL to revert the effect"""
        ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, None)


def stop_thread(thread):
    _async_raise(thread.ident, SystemExit)


# 小车电机引脚定义
IN1 = 20
IN2 = 21
IN3 = 19
IN4 = 26
ENA = 16
ENB = 13

# 小车按键定义
key = 8

# 循迹红外引脚定义
# TrackSensorLeftPin1 TrackSensorLeftPin2 TrackSensorRightPin1 TrackSensorRightPin2
#      3                 5                  4                   18
TrackSensorLeftPin1 = 3  # 定义左边第一个循迹红外传感器引脚为3口
TrackSensorLeftPin2 = 5  # 定义左边第二个循迹红外传感器引脚为5口
TrackSensorRightPin1 = 4  # 定义右边第一个循迹红外传感器引脚为4口
TrackSensorRightPin2 = 18  # 定义右边第二个循迹红外传感器引脚为18口

# 设置GPIO口为BCM编码方式
GPIO.setmode(GPIO.BCM)

# 忽略警告信息
GPIO.setwarnings(False)


# 电机引脚初始化为输出模式
# 按键引脚初始化为输入模式
# 寻迹引脚初始化为输入模式

def bgr8_to_jpeg(value, quality=75):
    return bytes(cv2.imencode('.jpg', value)[1])

def init():
    global pwm_ENA
    global pwm_ENB
    GPIO.setup(ENA, GPIO.OUT, initial=GPIO.HIGH)
    GPIO.setup(IN1, GPIO.OUT, initial=GPIO.LOW)
    GPIO.setup(IN2, GPIO.OUT, initial=GPIO.LOW)
    GPIO.setup(ENB, GPIO.OUT, initial=GPIO.HIGH)
    GPIO.setup(IN3, GPIO.OUT, initial=GPIO.LOW)
    GPIO.setup(IN4, GPIO.OUT, initial=GPIO.LOW)
    GPIO.setup(key, GPIO.IN)
    GPIO.setup(TrackSensorLeftPin1, GPIO.IN)
    GPIO.setup(TrackSensorLeftPin2, GPIO.IN)
    GPIO.setup(TrackSensorRightPin1, GPIO.IN)
    GPIO.setup(TrackSensorRightPin2, GPIO.IN)
    # 设置pwm引脚和频率为2000hz
    pwm_ENA = GPIO.PWM(ENA, 2000)
    pwm_ENB = GPIO.PWM(ENB, 2000)
    pwm_ENA.start(0)
    pwm_ENB.start(0)


# 小车前进
def run(leftspeed, rightspeed):
    GPIO.output(IN1, GPIO.HIGH)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.HIGH)
    GPIO.output(IN4, GPIO.LOW)
    pwm_ENA.ChangeDutyCycle(leftspeed)
    pwm_ENB.ChangeDutyCycle(rightspeed)


# 小车后退
def back(leftspeed, rightspeed):
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.HIGH)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.HIGH)
    pwm_ENA.ChangeDutyCycle(leftspeed)
    pwm_ENB.ChangeDutyCycle(rightspeed)


# 小车左转
def left(leftspeed, rightspeed):
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.HIGH)
    GPIO.output(IN4, GPIO.LOW)
    pwm_ENA.ChangeDutyCycle(leftspeed)
    pwm_ENB.ChangeDutyCycle(rightspeed)


# 小车右转
def right(leftspeed, rightspeed):
    GPIO.output(IN1, GPIO.HIGH)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.LOW)
    pwm_ENA.ChangeDutyCycle(leftspeed)
    pwm_ENB.ChangeDutyCycle(rightspeed)


def runone(delaytime):
    GPIO.output(IN1, GPIO.HIGH)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.HIGH)
    GPIO.output(IN4, GPIO.LOW)
    pwm_ENA.ChangeDutyCycle(35)
    pwm_ENB.ChangeDutyCycle(35)
    time.sleep(delaytime)


# 小车后退
def backone(delaytime):
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.HIGH)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.HIGH)
    pwm_ENA.ChangeDutyCycle(35)
    pwm_ENB.ChangeDutyCycle(35)
    time.sleep(delaytime)


# 小车原地左转
def spin_left(leftspeed, rightspeed):
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.HIGH)
    GPIO.output(IN3, GPIO.HIGH)
    GPIO.output(IN4, GPIO.LOW)
    pwm_ENA.ChangeDutyCycle(leftspeed)
    pwm_ENB.ChangeDutyCycle(rightspeed)


# 小车原地右转
def spin_right(leftspeed, rightspeed):
    GPIO.output(IN1, GPIO.HIGH)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.HIGH)
    pwm_ENA.ChangeDutyCycle(leftspeed)
    pwm_ENB.ChangeDutyCycle(rightspeed)


# 小车停止
def brake():
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.LOW)

# 按键检测
def key_scan():
    while GPIO.input(key):
        pass
    while not GPIO.input(key):
        time.sleep(0.01)
        if not GPIO.input(key):
            time.sleep(0.01)
        while not GPIO.input(key):
            pass


class ShapeDetector:
    def __init__(self):
        pass

    # 轮廓形状识别器 只有一个参数 c：轮廓
    # 为了进行形状检测，我们将使用轮廓近似法。 顾名思义，轮廓近似（contour approximation）是一种算法，用于通过减少一组点来减少曲线中的点数，因此称为术语近似。
    # 轮廓近似是基于以下假设：一条曲线可以由一系列短线段近似。这将导致生成近似曲线，该曲线由原始曲线定义的点子集组成。
    # 轮廓近似实际上已经通过cv2.approxPolyDP在OpenCV中实现。

    def detect(self, c):
        # 初始化形状名称，使用轮廓近似法
        # 计算周长
        peri = cv2.arcLength(c, True)
        approx = cv2.approxPolyDP(c, 0.04 * peri, True)

        # 轮廓是由一系列顶点组成的；如果是三角形，将拥有3个向量
        if len(approx) == 3:
            shape = "triangle"
        # 如果有4个顶点，那么是矩形或者正方形
        elif len(approx) == 4:
            # 计算轮廓的边界框 并且计算宽高比
            (x, y, w, h) = cv2.boundingRect(approx)
            ar = w / float(h)
            # 正方形的宽高比~~1 ，否则是矩形
            shape = "square" if 0.95 <= ar <= 1.05 else "rectangle"
        # 否则，根据上边的膨胀腐蚀，我们假设它为圆形
        else:
            shape = "circle"
        # 返回形状的名称
        return shape


# red1
r_low_hsv1 = np.array([160, 43, 60])
r_high_hsv1 = np.array([180, 255, 255])
# red2
r_low_hsv2 = np.array([0, 43, 60])
r_high_hsv2 = np.array([10, 255, 255])
# green
g_low_hsv1 = np.array([50, 43, 60])
g_high_hsv1 = np.array([94, 255, 255])
# blue
b_low_hsv1 = np.array([100, 43, 60])
b_high_hsv1 = np.array([124, 255, 255])
# yellow
y_low_hsv1 = np.array([11, 43, 60])
y_high_hsv1 = np.array([31, 255, 255])
font = cv2.FONT_HERSHEY_SIMPLEX

global is_detected


def Color_Recongnize():
    global is_detected
    while (1):

        # get a frame and show 获取视频帧并转成HSV格式, 利用cvtColor()将BGR格式转成HSV格式，参数为cv2.COLOR_BGR2HSV。
        ret, frame = cap.read()

        # cv2.imshow('Capture', frame)
        origin_widget.value = bgr8_to_jpeg(frame)

        # change to hsv model
        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

        mask_red = cv2.inRange(hsv, lowerb=r_low_hsv1, upperb=r_high_hsv1) + cv2.inRange(hsv, lowerb=r_low_hsv2,
                                                                                         upperb=r_high_hsv2)
        mask_blue = cv2.inRange(hsv, lowerb=b_low_hsv1, upperb=b_high_hsv1)
        mask_yellow = cv2.inRange(hsv, lowerb=y_low_hsv1, upperb=y_high_hsv1)
        mask_green = cv2.inRange(hsv, lowerb=g_low_hsv1, upperb=g_high_hsv1)

        inner_mask = [mask_yellow, mask_green]
        inner_mask_S = []
        inner_mask_counter = []
        outer_mask = [mask_blue + mask_yellow + mask_green, mask_red + mask_yellow + mask_green]
        outer_mask_S = []
        outer_mask_counter = []

        features = {"inner_color": "none", "inner_shape": "none", "outer_color": "none"}  # 内部颜色 内部形状 外部颜色 外部形状
        sd = ShapeDetector()
        detect = 0
        for i in range(len(inner_mask)):
            # 中值滤波降噪
            median = cv2.medianBlur(inner_mask[i], 7)
            k1 = np.ones((5, 5), np.uint8)
            opening = cv2.morphologyEx(median, cv2.MORPH_OPEN, k1)
            contours, hierarchy = cv2.findContours(median, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
            if len(contours) != 0:
                area = []
                # 找到最大的轮廓
                for k in range(len(contours)):
                    area.append(cv2.contourArea(contours[k]))
                max_idx = np.argmax(np.array(area))
                inner_mask_S.append(area[max_idx])
                inner_mask_counter.append(contours[max_idx])
            else:
                inner_mask_S.append(0)
                inner_mask_counter.append(None)
        inner_tag = inner_mask_S.index(max(inner_mask_S))
        if inner_tag == 0:
            features["inner_color"] = "yellow"
        else:
            features["inner_color"] = "green"
        if inner_mask_counter[inner_tag] is not None:
            shape = sd.detect(inner_mask_counter[inner_tag])
            if shape == "triangle":
                features["inner_shape"] = "triangle"
            elif shape == "circle":
                features["inner_shape"] = "circle"
        for i in range(len(outer_mask)):
            # 中值滤波降噪
            median = cv2.medianBlur(outer_mask[i], 7)
            k1 = np.ones((5, 5), np.uint8)
            opening = cv2.morphologyEx(median, cv2.MORPH_OPEN, k1)
            closing = cv2.morphologyEx(opening, cv2.MORPH_CLOSE, k1)
            contours, hierarchy = cv2.findContours(median, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
            if len(contours) != 0:
                area = []
                # 找到最大的轮廓
                for k in range(len(contours)):
                    area.append(cv2.contourArea(contours[k]))
                max_idx = np.argmax(np.array(area))
                outer_mask_S.append(area[max_idx])
                outer_mask_counter.append(contours[max_idx])
            else:
                outer_mask_S.append(0)
                outer_mask_counter.append(None)
        outer_tag = outer_mask_S.index(max(outer_mask_S))
        if outer_tag == 0:
            features["outer_color"] = "blue"
        else:
            features["outer_color"] = "red"
        if inner_mask_counter[inner_tag] is not None:
            M = cv2.moments(inner_mask_counter[inner_tag])  # 求矩
            if M['m00'] != 0:
                cx = int(M['m10'] / M['m00'])  # 求x坐标
                cy = int(M['m01'] / M['m00'])  # 求y坐标
                x, y, w, h = cv2.boundingRect(outer_mask_counter[outer_tag])
                if x < cx < x + w and y < cy < y + w:
                    if 0.05 < (inner_mask_S[inner_tag] / outer_mask_S[outer_tag]) < 0.3:
                        # 蓝真
                        if features["inner_color"] == "yellow" and features["inner_shape"] == "circle" and features[
                            "outer_color"] == "blue":
                            detect = 1
                        # 蓝假
                        elif features["inner_color"] == "green" and features["inner_shape"] == "triangle" and features[
                            "outer_color"] == "blue":
                            detect = 2
                        # 红真
                        elif features["inner_color"] == "yellow" and features["inner_shape"] == "circle" and features[
                            "outer_color"] == "red":
                            detect = 3
                        # 红假
                        elif features["inner_color"] == "green" and features["inner_shape"] == "triangle" and features[
                            "outer_color"] == "red":
                            detect = 4
        if detect == 1:
            print("true")
            is_detected = 1
        if detect == 2 or detect == 3 or detect == 4:
            print("false")
            is_detected = 0
        time.sleep(0.01)

    cap.release()
    # cv2.destroyAllWindows()

# 小车电机引脚定义
IN1 = 20
IN2 = 21
IN3 = 19
IN4 = 26
ENA = 16
ENB = 13

# 小车按键定义
key = 8

# 循迹红外引脚定义
# TrackSensorLeftPin1 TrackSensorLeftPin2 TrackSensorRightPin1 TrackSensorRightPin2
#      3                 5                  4                   18
TrackSensorLeftPin1 = 3  # 定义左边第一个循迹红外传感器引脚为3口
TrackSensorLeftPin2 = 5  # 定义左边第二个循迹红外传感器引脚为5口
TrackSensorRightPin1 = 4  # 定义右边第一个循迹红外传感器引脚为4口
TrackSensorRightPin2 = 18  # 定义右边第二个循迹红外传感器引脚为18口

# 设置GPIO口为BCM编码方式
GPIO.setmode(GPIO.BCM)
# 忽略警告信息
GPIO.setwarnings(False)

# 电机引脚初始化为输出模式
# 按键引脚初始化为输入模式
# 寻迹引脚初始化为输入模式
init()

origin_widget = widgets.Image(format='jpeg', width=320, height=240)
mask_widget = widgets.Image(format='jpeg', width=320, height=240)
result_widget = widgets.Image(format='jpeg', width=320, height=240)
image_container = widgets.HBox([origin_widget, mask_widget, result_widget])
display(image_container)

cap = cv2.VideoCapture(0)
cap.set(3, 640)
cap.set(4, 480)
cap.set(5, 120)  # 设置帧率
cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc('M', 'J', 'P', 'G'))
cap.set(cv2.CAP_PROP_BRIGHTNESS, 20)  # 设置亮度 -64 - 64  0.0
cap.set(cv2.CAP_PROP_CONTRAST, 20)  # 设置对比度 -64 - 64  2.0

# 代码二中的其他部分...
# 延时2s
time.sleep(2)
all_directions = [turn_directions0, turn_directions1, turn_directions2, turn_directions3, turn_directions4,
                  turn_directions5, turn_directions6, turn_directions7, turn_directions8]

# try/except语句用来检测try语句块中的错误，

# 从而让except语句捕获异常信息并处理。

# try/except语句用来检测try语句块中的错误，
try:
    # 运行代码一的main()函数
    main()
    init()
    key_scan()
    global is_detected
    i = 0
    while (i <= len(all_directions)):
        total_crossroads = len(all_directions[i])
        # 路径规划转化为小车行进
        current_crossroad = 0
        while current_crossroad < total_crossroads:
            # 循迹 未遇到路口直行，遇到路口跳出下循环
            while True:
                TrackSensorLeftValue1 = GPIO.input(TrackSensorLeftPin1)
                TrackSensorLeftValue2 = GPIO.input(TrackSensorLeftPin2)
                TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
                TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)
                # 循迹部分
                if (TrackSensorLeftValue1 == False or TrackSensorRightValue2 == False):
                    break
                elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
                    run(20, 20)  # 14
                    # 路口判断
                elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == True:
                    left(0, 35)
                elif TrackSensorLeftValue2 == True and TrackSensorRightValue1 == False:
                    right(35, 0)
            start_time = time.time()
            # 路口转弯
            while True:
                TrackSensorLeftValue1 = GPIO.input(TrackSensorLeftPin1)
                TrackSensorLeftValue2 = GPIO.input(TrackSensorLeftPin2)
                TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
                TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)
                if (all_directions[i][current_crossroad] == '0'):
                    if TrackSensorLeftValue1 == False:
                        spin_left(70, 70)  # 40
                    elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
                        run(20, 20)
                    elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == True:
                        left(0, 35)
                    elif TrackSensorLeftValue2 == True and TrackSensorRightValue1 == False:
                        right(35, 0)
                    current_time1 = time.time()
                    if current_time1 - start_time >= 1:
                        break
                elif (all_directions[i][current_crossroad] == '1'):
                    if TrackSensorRightValue2 == False:
                        spin_right(70, 70)
                    elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
                        run(20, 20)
                    elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == True:
                        left(0, 35)
                    elif TrackSensorLeftValue2 == True and TrackSensorRightValue1 == False:
                        right(35, 0)
                    current_time2 = time.time()
                    if current_time2 - start_time >= 1:
                        break

                elif (all_directions[i][current_crossroad] == '3'):
                    if TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
                        run(20, 20)
                    elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == True:
                        left(0, 35)
                    elif TrackSensorLeftValue2 == True and TrackSensorRightValue1 == False:
                        right(35, 0)
                    current_time3 = time.time()
                    if current_time3 - start_time >= 0.3:
                        break
            current_crossroad += 1
        # 颜色识别和撞击宝藏
        thread1 = threading.Thread(target=Color_Recongnize)
        thread1.setDaemon(True)
        thread1.start()
        while True:
            print('no')
            while True:
                TrackSensorLeftValue1 = GPIO.input(TrackSensorLeftPin1)
                TrackSensorLeftValue2 = GPIO.input(TrackSensorLeftPin2)
                TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
                TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)
                if TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
                    run(5, 5)  # 14
                elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == True:
                    left(0, 35)
                elif TrackSensorLeftValue2 == True and TrackSensorRightValue1 == False:
                    right(35, 0)
                if ((is_detected == 1) or (is_detected == 0)):
                    break
            if is_detected == 1:
                print('true')
                runone(0.2)
                backone(0.4)
                start_time1_1 = time.time()
                while True:
                    TrackSensorLeftValue1 = GPIO.input(TrackSensorLeftPin1)
                    TrackSensorLeftValue2 = GPIO.input(TrackSensorLeftPin2)
                    TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
                    TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)
                    spin_left(40, 40)
                    current_time1_1 = time.time()
                    if current_time1_1 - start_time1_1 >= 0.87:
                        break
                start_time1_2 = time.time()
                while True:
                    TrackSensorLeftValue1 = GPIO.input(TrackSensorLeftPin1)
                    TrackSensorLeftValue2 = GPIO.input(TrackSensorLeftPin2)
                    TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
                    TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)
                    if TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
                        run(7, 7)  # 14
                    elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == True:
                        left(0, 35)
                    elif TrackSensorLeftValue2 == True and TrackSensorRightValue1 == False:
                        right(35, 0)
                    current_time1_2 = time.time()
                    if current_time1_2 - start_time1_2 >= 0.4:
                        break
            elif is_detected == 0:
                print('false')
                start_time2_1 = time.time()
                backone(0.1)
                while True:
                    TrackSensorLeftValue1 = GPIO.input(TrackSensorLeftPin1)
                    TrackSensorLeftValue2 = GPIO.input(TrackSensorLeftPin2)
                    TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
                    TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)
                    spin_left(40, 40)
                    current_time2_1 = time.time()
                    if current_time2_1 - start_time2_1 >= 0.87:
                        break
                start_time2_2 = time.time()
                while True:
                    TrackSensorLeftValue1 = GPIO.input(TrackSensorLeftPin1)
                    TrackSensorLeftValue2 = GPIO.input(TrackSensorLeftPin2)
                    TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
                    TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)
                    if TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
                        run(5, 5)  # 14
                    elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == True:
                        left(0, 35)
                    elif TrackSensorLeftValue2 == True and TrackSensorRightValue1 == False:
                        right(35, 0)
                    current_time2_2 = time.time()
                    if current_time2_2 - start_time1_2 >= 0.4:
                        break
            break

        i = i + 1
except KeyboardInterrupt:
    pass
pwm_ENA.stop()
pwm_ENB.stop()
GPIO.cleanup()

